import visual
import math

class Arm :

    def __init__(self,
                 armlength = 2,
                 forearmlength = 2,
                 wristlength = 0.1,
                 wristrotatelength = 0.2,
                 griplength = 0.3) :

        self.table = visual.box( pos=(0,0,-0.25), height=10, width=10, length=0.5, axis=(0,0,-0.5) )

        self.basef = visual.frame( pos = (0,0,0), axis=(0,0,1) )
        self.basef.command = 1500
        
        self.base = visual.box( pos=(0.5,0,0),
                                height=1,
                                width=1,
                                length=1,
                                axis=(1,0,0),
                                
                                color=visual.color.red,
                                frame = self.basef,
                                )

        self.shoulderf = visual.frame( pos=(1,0,0), axis=(1,0,0), frame = self.basef )
        self.shoulderf.command = 1500

        self.arm = visual.cylinder(frame = self.shoulderf,
                                   axis = (armlength, 0,0, ),
                                   radius = 0.1,
                                   color=visual.color.blue )
            
        self.elbowf = visual.frame( pos=(armlength, 0,0), frame = self.shoulderf )
        self.elbowf.command = 1500
    
        self.forearm = visual.cylinder( frame = self.elbowf,
                                        axis = (forearmlength, 0,0 ),
                                        radius = 0.1,
                                        color=visual.color.green )

        self.wristf = visual.frame( pos=(forearmlength,0,0), frame = self.elbowf )
        self.wristf.command = 1500
        
        self.wrist = visual.cylinder( pos=(0,0,0),
                                      axis=(wristlength, 0,0),
                                      radius = 0.1,
                                      frame = self.wristf,
                                      color = visual.color.magenta
                                      )
        
        self.wristrotatef = visual.frame( pos=(wristlength, 0,0),
                                          frame = self.wristf )
        self.wristrotatef.command = 1500

        self.grip = visual.box( pos=( wristrotatelength / 2, 0, 0 ),
                                axis = ( 1,0,0 ),
                                height = 0.1,
                                width = 0.2,
                                length = wristrotatelength,
                                frame = self.wristrotatef
                                )
        
        self.gripf = visual.frame( pos=( wristrotatelength, 0, 0 ),
                                   frame = self.wristrotatef )
        self.gripf.command = 2500

        self.finger1 = visual.cylinder( pos = ( 0, 0, 0.1 ),
                                        radius = 0.04,
                                        axis = ( griplength, 0, 0),
                                        frame = self.gripf,
                                        color = visual.color.blue
                                        )

        self.finger2 = visual.cylinder( pos = ( 0, 0, -0.1 ),
                                        radius = 0.04,
                                        axis = ( griplength, 0, 0),
                                        frame = self.gripf,
                                        color = visual.color.blue
                                        )
    def rotatecmd( self, frame, command ) :
        if command < 500 :
            command = 500
        elif command > 2500 :
            command = 2500
            
        delta = command - frame.command
        angle = delta / 2000 * math.pi

        frame.rotate( angle = angle, axis = (0,0,1) )
        frame.command = command
        
    def cbase( self, command ) :
        self.rotatecmd( self.basef, command )
        
    def cshoulder( self, command ) :
        self.rotatecmd( self.shoulderf, command )
        
    def celbow( self, command ) :
        self.rotatecmd( self.elbowf, command )
        
    def cwrist( self, command ) :
        self.rotatecmd( self.wristf, command )
        
    def cwristrotate( self, command ) :
        self.rotatecmd( self.wristrotatef, command )

    def cgrip( self, command ) :
        if command < 500 :
            command = 500
        elif command > 2500 :
            command = 2500
            
        rel = (command - 500) / 2000

        pos = 0.04 + rel * (1.0 - 0.04)

        self.finger1.z = pos
        self.finger2.z = -pos
        
        self.gripf.command = command

a = Arm()
